`include "cpu_def.vh"

`define INDEX    {index_p, 27'd0, index_index}
`define RANDOM   {28'd0, random_random}
`define ENTRYLO0 {6'd0, entrylo0_fpn, entrylo0_c, entrylo0_d, entrylo0_v, entrylo0_g}
`define ENTRYLO1 {6'd0, entrylo1_fpn, entrylo1_c, entrylo1_d, entrylo1_v, entrylo1_g}
`define CONTEXT  {context_ptebase, context_badvpn2, 4'd0}
`define PAGEMASK {3'd0, pagemask_mask, 13'd0}
`define WIRED    {28'd0, wired_wired}
`define BADVADDR badvaddr
`define COUNT    count
`define ENTRYHI  {entryhi_vpn2, 5'd0, entryhi_asid}
`define COMPARE  compare
`define STATUS   {9'd0, status_bev, 6'd0, status_im, 3'd0, status_um, 2'd0, status_exl, status_ie}
`define CAUSE    {cause_bd, cause_ti, 14'd0, cause_ip, 1'd0, cause_exccode, 2'd0}
`define EPC      epc
`define EBASE    {2'b10, ebase_ebase, 2'd0, ebase_cpunum}
`define CONFIG0  {config0_m, 15'd0, config0_be, config0_at, config0_ar, config0_mt, 4'h0, config0_k0}
`define CONFIG1  {config1_m, config1_mmusize, config1_is, config1_il, config1_ia, config1_ds, config1_dl, config1_da, config1_c2, config1_md, config1_pc, config1_wr, config1_ca, config1_ep, config1_fp}

`define READ_INDEX    (cp0_rreg_num ==  0 && cp0_rsel == 0)
`define READ_RANDOM   (cp0_rreg_num ==  1 && cp0_rsel == 0)
`define READ_ENTRYLO0 (cp0_rreg_num ==  2 && cp0_rsel == 0)
`define READ_ENTRYLO1 (cp0_rreg_num ==  3 && cp0_rsel == 0)
`define READ_CONTEXT  (cp0_rreg_num ==  4 && cp0_rsel == 0)
`define READ_PAGEMASK (cp0_rreg_num ==  5 && cp0_rsel == 0)
`define READ_WIRED    (cp0_rreg_num ==  6 && cp0_rsel == 0)
`define READ_BADVADDR (cp0_rreg_num ==  8 && cp0_rsel == 0)
`define READ_COUNT    (cp0_rreg_num ==  9 && cp0_rsel == 0)
`define READ_ENTRYHI  (cp0_rreg_num == 10 && cp0_rsel == 0)
`define READ_COMPARE  (cp0_rreg_num == 11 && cp0_rsel == 0)
`define READ_STATUS   (cp0_rreg_num == 12 && cp0_rsel == 0)
`define READ_CAUSE    (cp0_rreg_num == 13 && cp0_rsel == 0)
`define READ_EPC      (cp0_rreg_num == 14 && cp0_rsel == 0)
`define READ_EBASE    (cp0_rreg_num == 15 && cp0_rsel == 1)
`define READ_CONFIG0  (cp0_rreg_num == 16 && cp0_rsel == 0)
`define READ_CONFIG1  (cp0_rreg_num == 16 && cp0_rsel == 1)

// cp0 write mask
`define INDEX_MASK    32'h0000000f
`define RANDOM_MASK   32'h00000000
`define ENTRYLO0_MASK 32'h03ffffff
`define ENTRYLO1_MASK 32'h03ffffff
`define CONTEXT_MASK  32'hff800000
`define PAGEMASK_MASK 32'h1fffe000
`define WIRED_MASK    32'h0000000f
`define BADVADDR_MASK 32'hffffffff
`define COUNT_MASK    32'hffffffff
`define ENTRYHI_MASK  32'hffffe0ff
`define COMPARE_MASK  32'hffffffff
`define STATUS_MASK   32'h0040ff03
`define CAUSE_MASK    32'hc000ff7c
`define EPC_MASK      32'hffffffff
`define EBASE_MASK    32'h3ffff000
`define CONFIG0_MASK  32'h00000007
`define CONFIG1_MASK  32'h00000000

`define WRITE_INDEX    (cp0_wreg_num ==  0 && cp0_wsel == 0) && cp0_wen
// `define WRITE_RANDOM   (cp0_wreg_num ==  1 && cp0_wsel == 0) && cp0_wen
`define WRITE_ENTRYLO0 (cp0_wreg_num ==  2 && cp0_wsel == 0) && cp0_wen
`define WRITE_ENTRYLO1 (cp0_wreg_num ==  3 && cp0_wsel == 0) && cp0_wen
`define WRITE_CONTEXT  (cp0_wreg_num ==  4 && cp0_wsel == 0) && cp0_wen
`define WRITE_PAGEMASK (cp0_wreg_num ==  5 && cp0_wsel == 0) && cp0_wen
`define WRITE_WIRED    (cp0_wreg_num ==  6 && cp0_wsel == 0) && cp0_wen
`define WRITE_COUNT    (cp0_wreg_num ==  9 && cp0_wsel == 0) && cp0_wen
`define WRITE_ENTRYHI  (cp0_wreg_num == 10 && cp0_wsel == 0) && cp0_wen
`define WRITE_COMPARE  (cp0_wreg_num == 11 && cp0_wsel == 0) && cp0_wen
`define WRITE_STATUS   (cp0_wreg_num == 12 && cp0_wsel == 0) && cp0_wen
`define WRITE_CAUSE    (cp0_wreg_num == 13 && cp0_wsel == 0) && cp0_wen
`define WRITE_EPC      (cp0_wreg_num == 14 && cp0_wsel == 0) && cp0_wen
`define WRITE_EBASE    (cp0_wreg_num == 15 && cp0_wsel == 1) && cp0_wen
`define WRITE_CONFIG0  (cp0_wreg_num == 16 && cp0_wsel == 0) && cp0_wen
// `define WRITE_CONFIG1  (cp0_wreg_num == 16 && cp0_wsel == 1) && cp0_wen

module cp0(
  (*mark_debug = "true"*)input clk,
  (*mark_debug = "true"*)input rst,

  // from outside
  input [5:0] ext_int,

  // from wb stage
  /* hardware write channel */
  output [31:0] cp0_ex_pc,

  input        bd            ,
  input        tlbre_taken   ,
  input [31:0] pc            ,
  input [31:0] addr          ,
  input        ex_taken      ,
  input [ 4:0] ex_code       ,
  input        eret_taken    ,
  input        tlbp_taken    ,
  input        tlbr_taken    ,
  /* software write channel */
  (*mark_debug = "true"*)input        cp0_wen     ,
  (*mark_debug = "true"*)input [ 4:0] cp0_wreg_num,
  (*mark_debug = "true"*)input [ 2:0] cp0_wsel    ,
  (*mark_debug = "true"*)input [31:0] cp0_wdata   ,

  // from de stage
  /* software read channel */
  (*mark_debug = "true"*)input [ 4:0] cp0_rreg_num,
  (*mark_debug = "true"*)input [ 2:0] cp0_rsel    ,

  // to de stage
  /* software read channel */
  (*mark_debug = "true"*)output [31:0] cp0_rdata,
  (*mark_debug = "true"*)output [31:0] cp0_rmask,
  output [31:0] cp0_epc  ,
  output        has_int  ,

  // to mmu
  /* hardware read channel */
  output [31:0] cp0_index_rdata   ,
  output [31:0] cp0_random_rdata  ,
  output [31:0] cp0_entrylo0_rdata,
  output [31:0] cp0_entrylo1_rdata,
  output [31:0] cp0_entryhi_rdata ,
  output [31:0] cp0_config0_rdata ,

  // from mmu
  /* hardware write channel */
  input [31:0] cp0_index_wdata   ,
  input [31:0] cp0_entrylo0_wdata,
  input [31:0] cp0_entrylo1_wdata,
  input [31:0] cp0_entryhi_wdata 
);

  reg       index_p;
  reg [3:0] index_index;

  reg [3:0] random_random;

  reg [19:0] entrylo0_fpn;
  reg [ 2:0] entrylo0_c;
  reg        entrylo0_d;
  reg        entrylo0_v;
  reg        entrylo0_g;
  
  reg [19:0] entrylo1_fpn;
  reg [ 2:0] entrylo1_c;
  reg        entrylo1_d;
  reg        entrylo1_v;
  reg        entrylo1_g;

  reg [ 8:0] context_ptebase;
  reg [18:0] context_badvpn2;

  reg [15:0] pagemask_mask;

  reg [3:0] wired_wired;

  reg [31:0] badvaddr;

  reg        tick;
  (*mark_debug = "true"*)reg [31:0] count;

  reg [18:0] entryhi_vpn2;
  reg [ 7:0] entryhi_asid;

  reg [31:0] compare;

  reg       status_bev;
  reg [7:0] status_im ;
  reg       status_um ;
  reg       status_exl;
  reg       status_ie ;

  reg       cause_bd;
  reg       cause_ti;
  reg [7:0] cause_ip;
  reg [4:0] cause_exccode;

  reg [31:0] epc;

  reg [17:0] ebase_ebase;
  reg [9:0] ebase_cpunum;

  reg        config0_m;
  reg        config0_be;
  reg [ 1:0] config0_at;
  reg [ 2:0] config0_ar;
  reg [ 2:0] config0_mt;
  reg [ 2:0] config0_k0;

  reg       config1_m;
  reg [6:0] config1_mmusize;
  reg [2:0] config1_is;
  reg [2:0] config1_il;
  reg [2:0] config1_ia;
  reg [2:0] config1_ds;
  reg [2:0] config1_dl;
  reg [2:0] config1_da;
  reg       config1_c2;
  reg       config1_md;
  reg       config1_pc;
  reg       config1_wr;
  reg       config1_ca;
  reg       config1_ep;
  reg       config1_fp;

  wire [31:0] ex_base;
  wire [11:0] ex_offset;

  always@(posedge clk) begin
    if (rst) begin
      index_p <= 1'b0;
    end else if (tlbp_taken) begin  // note: index.p cannot be written by mtc0
      index_p <= cp0_index_wdata[31];
    end
  end
  always@(posedge clk) begin
    if (tlbp_taken) begin
      index_index <= cp0_index_wdata[3:0];
    end else if (`WRITE_INDEX) begin
      index_index <= cp0_wdata[3:0];
    end
  end

  always@(posedge clk) begin
    if (rst && `WRITE_WIRED) begin
      random_random <= 4'hf;
    end else begin
      if (random_random == wired_wired) begin
        random_random <= 4'hf;
      end else begin
        random_random <= random_random - 1;
      end
    end
  end

  always@(posedge clk) begin
     if (tlbr_taken) begin
      entrylo0_fpn <= cp0_entrylo0_wdata[25:6];
      entrylo0_c   <= cp0_entrylo0_wdata[ 5:3];
      entrylo0_d   <= cp0_entrylo0_wdata[   2];
      entrylo0_v   <= cp0_entrylo0_wdata[   1];
      entrylo0_g   <= cp0_entrylo0_wdata[   0];
     end else if (`WRITE_ENTRYLO0) begin
      entrylo0_fpn <= cp0_wdata[25:6];
      entrylo0_c   <= cp0_wdata[ 5:3];
      entrylo0_d   <= cp0_wdata[   2];
      entrylo0_v   <= cp0_wdata[   1];
      entrylo0_g   <= cp0_wdata[   0];
    end
  end

  always@(posedge clk) begin
    if (tlbr_taken) begin
      entrylo1_fpn <= cp0_entrylo1_wdata[25:6];
      entrylo1_c   <= cp0_entrylo1_wdata[ 5:3];
      entrylo1_d   <= cp0_entrylo1_wdata[   2];
      entrylo1_v   <= cp0_entrylo1_wdata[   1];
      entrylo1_g   <= cp0_entrylo1_wdata[   0];
    end else if (`WRITE_ENTRYLO1) begin
      entrylo1_fpn <= cp0_wdata[25:6];
      entrylo1_c   <= cp0_wdata[ 5:3];
      entrylo1_d   <= cp0_wdata[   2];
      entrylo1_v   <= cp0_wdata[   1];
      entrylo1_g   <= cp0_wdata[   0];
    end
  end

  always@(posedge clk) begin
    if (`WRITE_CONTEXT) begin
      context_ptebase <= cp0_wdata[31:23];
    end
  end
  always@(posedge clk) begin
    if (
      ex_taken && 
      (ex_code == `EX_TLBL || 
       ex_code == `EX_TLBS ||
       ex_code == `EX_TLBM)
    ) begin
      context_badvpn2 <= addr[31:13];
    end
  end

  always@(posedge clk) begin
    if (rst) begin
      pagemask_mask <= 0;
    end else if (`WRITE_PAGEMASK) begin
      pagemask_mask <= cp0_wdata[28:13];
    end
  end

  always@(posedge clk) begin
    if (rst) begin
      wired_wired <= 0;
    end else if (`WRITE_WIRED) begin
      wired_wired <= cp0_wdata[3:0];
    end
  end

  always@(posedge clk) begin
    if (
      ex_taken && 
      (ex_code == `EX_ADEL || 
       ex_code == `EX_ADES || 
       ex_code == `EX_TLBL || 
       ex_code == `EX_TLBS ||
       ex_code == `EX_TLBM)
    ) begin
      badvaddr <= addr;
    end
  end

  always@(posedge clk) begin
    if (rst) begin
      tick <= 1'b0;
    end else begin
      tick <= ~tick;
    end

    if (rst) begin
      count <= 32'd0;
    end else if (`WRITE_COUNT) begin
      count <= cp0_wdata;
    end else if (tick) begin
      count <= count + 1'b1;
    end
  end

  always@(posedge clk) begin
    if (ex_taken && (ex_code == `EX_TLBL || ex_code == `EX_TLBS || ex_code == `EX_TLBM)) begin
      entryhi_vpn2 <= addr[31:13];
    end else if (tlbr_taken) begin
      entryhi_vpn2 <= cp0_entryhi_wdata[31:13];
    end else if (`WRITE_ENTRYHI) begin
      entryhi_vpn2 <= cp0_wdata[31:13];
    end
  end
  always@(posedge clk) begin
    if (rst) begin
      entryhi_asid <= 0;
    end else if (tlbr_taken) begin
      entryhi_asid <= cp0_entryhi_wdata[ 7: 0];
    end else if (`WRITE_ENTRYHI) begin
      entryhi_asid <= cp0_wdata[ 7: 0];
    end
  end

  always@(posedge clk) begin
    if (`WRITE_COMPARE) begin
      compare <= cp0_wdata;
    end
  end

  always@(posedge clk) begin
    if (rst) begin
      status_bev <= 1'b1;
    end else if (`WRITE_STATUS) begin
      status_bev <= cp0_wdata[22];
    end
  end
  always@(posedge clk) begin
    if (rst) begin
      status_im <= 8'd0;
    end else if (`WRITE_STATUS) begin
      status_im <= cp0_wdata[15:8];
    end
  end
  always@(posedge clk) begin
    if (`WRITE_STATUS) begin
      status_um <= cp0_wdata[4];
    end
  end
  always@(posedge clk) begin
    if (rst) begin
      status_exl <= 1'b0;
    end else if (ex_taken) begin
      status_exl <= 1'b1;
    end else if (eret_taken) begin
      status_exl <= 1'b0;
    end else if (`WRITE_STATUS) begin
      status_exl <= cp0_wdata[1];
    end
  end
  always@(posedge clk) begin
    if (rst) begin
      status_ie <= 1'b0;
    end else if (`WRITE_STATUS) begin
      status_ie <= cp0_wdata[0];
    end
  end

  always@(posedge clk) begin
    if (rst) begin
      cause_bd <= 1'b0;
    end else if (ex_taken && !status_exl) begin
      cause_bd <= bd;
    end
  end
  always@(posedge clk) begin
    if (rst) begin
      cause_ti <= 1'b0;
    end else if (`WRITE_COMPARE) begin
      cause_ti <= 1'b0;
    end else if (count == compare) begin
      cause_ti <= 1'b1;
    end
  end
  always@(posedge clk) begin
    if (rst) begin
      cause_ip[7:2] <= 6'd0;
    end else begin
      cause_ip[7  ] <= ext_int[5] | cause_ti;
      cause_ip[6:2] <= ext_int[4:0];
    end
  end
  always@(posedge clk) begin
    if (rst) begin
      cause_ip[1:0] <= 2'd0;
    end else if (`WRITE_CAUSE) begin
      cause_ip[1:0] <= cp0_wdata[9:8];
    end
  end
  always@(posedge clk) begin
    if (rst) begin
      cause_exccode <= 5'd0;
    end else if (ex_taken) begin
      cause_exccode <= ex_code;
    end
  end

  always@(posedge clk) begin
    if (ex_taken && !status_exl) begin
      epc <= bd ? pc - 3'h4 : pc;
    end else if (`WRITE_EPC) begin
      epc <= cp0_wdata;
    end
  end

  always@(posedge clk) begin
    if (rst) begin
      ebase_cpunum <= 0;
    end
  end
  always@(posedge clk) begin
    if (rst) begin
      ebase_ebase <= 0;
    end else if (`WRITE_EBASE) begin
      ebase_ebase <= cp0_wdata[29:12];
    end
  end

  always@(posedge clk) begin
    if (rst) begin
      config0_m <= 1'b1;
      config0_be <= 1'b0;
      config0_at <= 2'b00;
      config0_ar <= 3'b000;
      config0_mt <= 3'b001;
    end
  end
  always@(posedge clk) begin
    if (rst) begin
      // reset kseg0 uncache
      config0_k0 <= 3'b011;
    end else if (`WRITE_CONFIG0) begin
      config0_k0 <= cp0_wdata[2:0];
    end
  end

  always@(posedge clk) begin
    if (rst) begin
      config1_m <= 1'b0;
      config1_mmusize <= 15;
      config1_is <= 2;
      config1_il <= 3;
      config1_ia <= 1;
      config1_ds <= 2;
      config1_dl <= 3;
      config1_da <= 1;
      config1_c2 <= 0;
      config1_md <= 0;
      config1_pc <= 0;
      config1_wr <= 0;
      config1_ca <= 0;
      config1_ep <= 0;
      config1_fp <= 0;
    end
  end

  assign ex_base = status_bev ? 32'hbfc00200 : {2'b10, ebase_ebase, 12'h000};
  assign ex_offset = (tlbre_taken && !status_exl) ? 12'h000 : 12'h180;
  assign cp0_ex_pc = ex_base + ex_offset;

  assign cp0_rdata = 
    {32{`READ_INDEX   }} & `INDEX    |
    {32{`READ_RANDOM  }} & `RANDOM   |
    {32{`READ_ENTRYLO0}} & `ENTRYLO0 |
    {32{`READ_ENTRYLO1}} & `ENTRYLO1 |
    {32{`READ_CONTEXT }} & `CONTEXT  |
    {32{`READ_PAGEMASK}} & `PAGEMASK |
    {32{`READ_WIRED   }} & `WIRED    |
    {32{`READ_BADVADDR}} & `BADVADDR |
    {32{`READ_COUNT   }} & `COUNT    |
    {32{`READ_ENTRYHI }} & `ENTRYHI  |
    {32{`READ_COMPARE }} & `COMPARE  |
    {32{`READ_STATUS  }} & `STATUS   |
    {32{`READ_CAUSE   }} & `CAUSE    |
    {32{`READ_EPC     }} & `EPC      |
    {32{`READ_EBASE   }} & `EBASE    |
    {32{`READ_CONFIG0 }} & `CONFIG0  |
    {32{`READ_CONFIG1 }} & `CONFIG1  ;

  assign cp0_rmask = 
    {32{`READ_INDEX   }} & `INDEX_MASK    |
    {32{`READ_RANDOM  }} & `RANDOM_MASK   |
    {32{`READ_ENTRYLO0}} & `ENTRYLO0_MASK |
    {32{`READ_ENTRYLO1}} & `ENTRYLO1_MASK |
    {32{`READ_CONTEXT }} & `CONTEXT_MASK  |
    {32{`READ_PAGEMASK}} & `PAGEMASK_MASK |
    {32{`READ_WIRED   }} & `WIRED_MASK    |
    {32{`READ_BADVADDR}} & `BADVADDR_MASK |
    {32{`READ_COUNT   }} & `COUNT_MASK    |
    {32{`READ_ENTRYHI }} & `ENTRYHI_MASK  |
    {32{`READ_COMPARE }} & `COMPARE_MASK  |
    {32{`READ_STATUS  }} & `STATUS_MASK   |
    {32{`READ_CAUSE   }} & `CAUSE_MASK    |
    {32{`READ_EPC     }} & `EPC_MASK      |
    {32{`READ_EBASE   }} & `EBASE_MASK    |
    {32{`READ_CONFIG0 }} & `CONFIG0_MASK  |                  
    {32{`READ_CONFIG1 }} & `CONFIG1_MASK  ;                  
  assign cp0_epc = `EPC;
  assign has_int = |(cause_ip & status_im) && status_ie && !status_exl;

  assign cp0_index_rdata = `INDEX;
  assign cp0_random_rdata = `RANDOM;
  assign cp0_entrylo0_rdata = `ENTRYLO0;
  assign cp0_entrylo1_rdata = `ENTRYLO1;
  assign cp0_entryhi_rdata = `ENTRYHI;
  assign cp0_config0_rdata = `CONFIG0;

endmodule
